#ifndef SR300A_HPP
#define SR300A_HPP

#include "sr300.hpp"
#include <algorithm>

#include "ransacline.hpp"

class SR300A : public SR300
{

public:

    template<typename T>
    static void ToCoordinate(std::vector<T> &pt, const cv::Mat &bw)
    {
        for(int i=0;i<bw.rows;i++)
        {
            for(int j=0;j<bw.cols;j++)
            {
                if(bw.at<uchar>(i,j))
                    pt.push_back(T(j,i));
            }
        }
    }

    static void LocalMax2D(std::vector<cv::Point> &ptMax, const cv::Mat &m, cv::Size localSize, double thresholdRatio=0.3, bool bMax=true)
    {
        /// Localizing the best match with minMaxLoc
        double minVal; double maxVal; cv::Point minLoc; cv::Point maxLoc;

        cv::minMaxLoc( m, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat() );


        cv::Mat element = cv::getStructuringElement( cv::MORPH_RECT, localSize);
        cv::Mat mask;
        cv::Mat m2;
        if(bMax)
        {
            cv::dilate(m, m2, element);
            double threshold=maxVal-(maxVal-minVal)*thresholdRatio;
            mask=(m2==m)&(m2>threshold);
        }
        else
        {
            cv::erode(m, m2, element);
            double threshold=minVal+(maxVal-minVal)*thresholdRatio;
            mask=(m2==m)&(m2<threshold);
        }
        //     Display the image in GUI
        //    namedWindow("mask", WINDOW_AUTOSIZE );
        //    imshow("mask", mask);

        ToCoordinate(ptMax, mask);
    }


    static void TempMatch(std::vector<cv::Point> &ptMatch, const cv::Mat &m, const cv::Mat &templ, cv::Size localSize, double thresholdRatio=0.3, int match_method=cv::TM_SQDIFF)
    {

        cv::Mat res;
        cv::matchTemplate(m,templ,res,match_method);

        bool bMax=true;

        /// For SQDIFF and SQDIFF_NORMED, the best matches are lower values. For all the other methods, the higher the better
        if( match_method  == cv::TM_SQDIFF || match_method == cv::TM_SQDIFF_NORMED )
        {
            bMax=false;
        }

        LocalMax2D(ptMatch, res, localSize, thresholdRatio, bMax);

        //         std::cout<<"ptMatch="<<cv::Mat(ptMatch)<<"\n"<<std::flush;
        cv::Point offset(templ.cols/2, templ.rows/2);

        cv::Mat m3;
        cv::cvtColor(m,m3,CV_GRAY2BGR);

        for(auto &pt:ptMatch)
        {
            /// Show me what you got
            cv::rectangle( m3, pt, cv::Point( pt.x + templ.cols , pt.y + templ.rows ), cv::Scalar(0,0,255), 2, 8, 0 );
            pt+=offset;
        }

        //         std::cout<<"ptMatch="<<cv::Mat(ptMatch)<<"\n"<<std::flush;
        // Display the image in GUI
        cv::namedWindow("TempMatch", cv::WINDOW_AUTOSIZE );
        cv::imshow("TempMatch", m3);
        cv::waitKey();
    }


    static void DrawLine(cv::Mat &canvas, double a, double b, double c, cv::Scalar color=cv::Scalar(0,255,0))
    {
        cv::Point l[2]={
            cv::Point(-c/a,0),
            cv::Point(-(c+canvas.rows*b)/a, canvas.rows)
        };

        cv::line(canvas, l[0], l[1], color);
    }




    static void CrossLocate(std::vector<cv::Vec3f> &cross, const cv::Mat &xyz, double targetZ, double thickness, cv::Size localSize, double thresholdRatio=0.3, int match_method=cv::TM_SQDIFF)
    {
        std::vector<cv::Mat> axis;
        cv::split(xyz, axis);

        cv::Mat bw;
        cv::inRange(axis[2], targetZ-thickness*2, targetZ+thickness*2, bw);

        //        cv::threshold(axis[2], axis[2], targetZ-thickness*2, 0, cv::THRESH_TOZERO);
        //        cv::threshold(axis[2], axis[2], targetZ+thickness*2, 0, cv::THRESH_TOZERO_INV);

#if 0
        double rad;

        {

            std::vector<cv::Vec2f> ptt;
            ToCoordinate(ptt, bw);

            double acceptPercentage=0.2;
            double errorThreshold=4;

            RansacLine rsl;
            rsl.Setup(ptt, acceptPercentage, errorThreshold);
            size_t n;
            rsl.Fit(n);
            bool *inliner = new bool[ptt.size()];
            double *error=new double[ptt.size()];
            rsl.GetInOutFlag(inliner, error);

            rad=atan2(-rsl.model[0], rsl.model[1]);

            std::cout<<"line1="<<rsl.model<<"\n";

            std::cout<<"la1="<<-rad<<"\n"<<std::flush;

            //        DrawLine(bw, rsl.model[0], rsl.model[1], rsl.model[2]);
        }

#endif

        int thickPixel=11;

        cv::Mat temp(5*thickPixel,5*thickPixel,CV_8UC1,cv::Scalar(0));
        temp.rowRange(2*thickPixel,3*thickPixel).setTo(255);
        temp.colRange(2*thickPixel,3*thickPixel).setTo(255);

        std::vector<cv::Point> ptMatch;

        TempMatch(ptMatch, bw, temp, localSize, thresholdRatio, match_method);



        cross.clear();

        for(auto pt:ptMatch)
        {
            cross.push_back(xyz.at<cv::Vec3f>(pt));
        }
    }

    static void test()
    {
        cv::Mat xyz=cv::imread("xyz.hdr",cv::IMREAD_UNCHANGED);
        std::vector<cv::Vec3f> cross;
        CrossLocate(cross, xyz, 0.37, 0.01, cv::Size(130,130), 0.05);
        std::cout<<"cross="<<cv::Mat(cross)<<"\n"<<std::flush;
        cv::waitKey();
    }

    static void Rotate(cv::Mat &out, const cv::Mat &in, double rad)
    {
        cv::Size outsz=cv::RotatedRect(cv::Point2f(0,0),cv::Size2f(in.cols,in.rows),rad*180/CV_PI).boundingRect().size();
        cv::Matx22d rm2(cos(rad),sin(rad),-sin(rad),cos(rad));
        cv::Vec2d offset=cv::Vec2d(outsz.width,outsz.height)-rm2*cv::Vec2d(in.cols,in.rows);
        cv::Matx23d rm;
        cv::hconcat(rm2,offset/2,rm);
        cv::warpAffine(in, out, rm, outsz);
    }

public:
    SR300A()
        : SR300()
    {

    }

    virtual void ProcessFrame(const cv::Mat &color, const cv::Mat &ir, const cv::Mat &depth, const cv::Mat &xyz, const cv::Mat &texture, const cv::Mat &aligned_depth, const cv::Mat &aligned_xyz)
    {
        SR300::ProcessFrame(color, ir, depth, xyz, texture, aligned_depth, aligned_xyz);
        std::vector<cv::Vec3f> cross;
        CrossLocate(cross, xyz, 0.37, 0.01, cv::Size(130,130), 0.05);
        std::cout<<"cross="<<cv::Mat(cross)<<"\n"<<std::flush;
    }


};

#endif
